Skip to content

feat: keyboard mapping to velocity and pose via existing interfaces (#1159)#1550

Draft
christiefhyang wants to merge 7 commits intodevfrom
christie/feat-keyboard-control
Draft

feat: keyboard mapping to velocity and pose via existing interfaces (#1159)#1550
christiefhyang wants to merge 7 commits intodevfrom
christie/feat-keyboard-control

Conversation

@christiefhyang
Copy link
Member

This PR adds keyboard-based control for both velocity and pose using the existing control/navigation interfaces for the Unitree G1.> It keeps the existing KeyboardTeleop for continuous cmd_vel control and adds a new module to send small relative pose goals through the navigation stack.

Details:

  • Velocity control
    • Reuse KeyboardTeleop to map keyboard input to Twist messages.
    • Commands are published to the existing /cmd_vel transport; no changes to the low-level control path.

  • Pose control
    • Add KeyboardPoseTeleop, a pygame-based module that:
    • Subscribes to odom: In[PoseStamped] for the current robot pose.
    • Maps key presses (Arrow keys, Q/E, Space) to relative motion (forward, left, degrees).
    • Generates a new PoseStamped goal in the same frame as odom and publishes it on goal_pose: Out[PoseStamped].
    • Sends cancel_goal: Out[Bool] on Space for goal cancellation.
    • This reuses the existing navigation topics /odom, /goal_pose, and /cancel_goal instead of introducing new control interfaces.

  • Blueprint wiring
    • Update unitree_g1_blueprints.with_joystick to include both:
    • keyboard_teleop() for velocity (cmd_vel).
    • keyboard_pose_teleop() for discrete pose goals.

Breaking Changes

None. This change only adds a new module and extends the G1 joystick blueprint; existing behavior is preserved.

How to Test

#In a supported Linux/WSL environment:
 uv sync --extra dev --extra cpu --extra sim
 uv run dimos --simulation run unitree-g1-joystick

• Focus the Keyboard Teleop window:
• W/S: forward/backward
• A/D: turn left/right
• Q/E: strafe left/right
• Shift / Ctrl: speed up / slow down
• Space: emergency stop (cmd_vel = 0)
• Focus the Keyboard Pose Teleop window:
• ArrowUp/ArrowDown: move forward/backward by a fixed step (relative to current heading).
• ArrowLeft/ArrowRight: move left/right by a fixed step.
• Q/E: rotate in place by a fixed yaw step.
• Space: cancel the current navigation goal.
• ESC: close the pose teleop window.

Contributor License Agreement

  • I have read and approved the CLA.

@greptile-apps
Copy link
Contributor

greptile-apps bot commented Mar 14, 2026

Greptile Summary

This PR adds a new KeyboardPoseTeleop module for sending discrete relative pose goals via keyboard input and a new monolithic blueprint file to wire it into the G1 joystick configuration.

  • KeyboardPoseTeleop (dimos/teleop/keyboard/keyboard_pose_teleop.py): Well-structured pygame-based module that subscribes to odometry, maps arrow keys / Q / E to relative pose offsets, and publishes PoseStamped goals through the existing navigation stack. The core goal-generation logic correctly uses quaternion rotation to transform local offsets into global frame coordinates.
  • Blueprint file (dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py): Contains a broken import (dimos.robot.unitree_webrtc.keyboard_pose_teleop) that will crash at import time — the module actually lives at dimos.teleop.keyboard.keyboard_pose_teleop. Additionally, this file re-declares every existing G1 blueprint in a single monolithic file, duplicating the established modular structure in basic/, agentic/, perceptive/, and primitive/ subdirectories. It is not registered in the __init__.py lazy loader, making it effectively unreachable through normal imports. The intended change (adding keyboard_pose_teleop to the joystick blueprint) would be better accomplished by editing the existing basic/unitree_g1_joystick.py.

Confidence Score: 2/5

  • This PR will fail at runtime due to a broken import path and introduces blueprint duplication that conflicts with the existing architecture.
  • Score of 2 reflects that while the core teleop module logic is sound, the blueprint file has a guaranteed ImportError from a wrong module path and introduces significant structural duplication of existing code without being wired into the module system.
  • dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py needs the most attention — fix the import path and ideally integrate the new teleop into the existing modular blueprint structure instead.

Important Files Changed

Filename Overview
dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py New monolithic blueprint file that duplicates the existing modular blueprint structure. Contains a broken import (dimos.robot.unitree_webrtc.keyboard_pose_teleop) that will cause an ImportError at runtime. Not wired into the existing __init__.py lazy loader.
dimos/teleop/keyboard/keyboard_pose_teleop.py New KeyboardPoseTeleop module for discrete pose goal control via keyboard. Follows existing patterns well (pygame loop, threading, Module base class). Minor style issue with start() return type. Core logic for relative goal generation is correct.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    subgraph KeyboardPoseTeleop
        PG[Pygame Key Event] --> HK{Key Pressed?}
        HK -->|Arrow Keys| MK[Map to forward/left offset]
        HK -->|Q/E| RK[Map to yaw delta]
        HK -->|Space| CG[Publish cancel_goal: Bool]
        HK -->|ESC| QUIT[Stop module]
        MK --> GEN[_generate_new_goal]
        RK --> GEN
    end

    subgraph Goal Generation
        ODOM[odom: In PoseStamped] -->|_on_odom| CP[_current_pose]
        CP --> GEN
        GEN --> ROT[Rotate local offset by current orientation]
        ROT --> ADD[Add to current position]
        ADD --> YAW[Apply yaw delta to current heading]
        YAW --> GOAL[New PoseStamped goal]
    end

    subgraph Navigation Stack
        GOAL -->|goal_pose: Out| GP[/goal_pose transport/]
        CG -->|cancel_goal: Out| CGT[/cancel_goal transport/]
    end
Loading

Last reviewed commit: 5e0afde

Comment on lines +1 to +270
#!/usr/bin/env python3
# Copyright 2025-2026 Dimensional Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Blueprint configurations for Unitree G1 humanoid robot.

This module provides pre-configured blueprints for various G1 robot setups,
from basic teleoperation to full autonomous agent configurations.
"""

from dimos_lcm.foxglove_msgs import SceneUpdate
from dimos_lcm.foxglove_msgs.ImageAnnotations import (
ImageAnnotations,
)
from dimos_lcm.sensor_msgs import CameraInfo

from dimos.agents.agent import llm_agent
from dimos.agents.cli.human import human_input
from dimos.agents.skills.navigation import navigation_skill
from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE
from dimos.core.blueprints import autoconnect
from dimos.core.transport import LCMTransport, pSHMTransport
from dimos.hardware.sensors.camera import zed
from dimos.hardware.sensors.camera.module import camera_module # type: ignore[attr-defined]
from dimos.hardware.sensors.camera.webcam import Webcam
from dimos.mapping.costmapper import cost_mapper
from dimos.mapping.voxels import voxel_mapper
from dimos.msgs.geometry_msgs import (
PoseStamped,
Quaternion,
Transform,
Twist,
Vector3,
)
from dimos.msgs.nav_msgs import Odometry, Path
from dimos.msgs.sensor_msgs import Image, PointCloud2
from dimos.msgs.std_msgs import Bool
from dimos.msgs.vision_msgs import Detection2DArray
from dimos.navigation.frontier_exploration import wavefront_frontier_explorer
from dimos.navigation.replanning_a_star.module import replanning_a_star_planner
from dimos.navigation.rosnav import ros_nav
from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector
from dimos.perception.detection.module3D import Detection3DModule, detection3d_module
from dimos.perception.detection.moduleDB import ObjectDBModule, detectionDB_module
from dimos.perception.detection.person_tracker import PersonTracker, person_tracker_module
from dimos.perception.object_tracker import object_tracking
from dimos.perception.spatial_perception import spatial_memory
from dimos.robot.foxglove_bridge import foxglove_bridge
from dimos.robot.unitree.connection.g1 import g1_connection
from dimos.robot.unitree.connection.g1sim import g1_sim_connection
from dimos.robot.unitree.g1.skill_container import g1_skills
from dimos.robot.unitree.keyboard_teleop import keyboard_teleop
from dimos.robot.unitree_webrtc.keyboard_pose_teleop import keyboard_pose_teleop
from dimos.utils.monitoring import utilization
from dimos.web.websocket_vis.websocket_vis_module import websocket_vis

_basic_no_nav = (
autoconnect(
camera_module(
transform=Transform(
translation=Vector3(0.05, 0.0, 0.0),
rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)),
frame_id="sensor",
child_frame_id="camera_link",
),
hardware=lambda: Webcam(
camera_index=0,
fps=15,
stereo_slice="left",
camera_info=zed.CameraInfo.SingleWebcam,
),
),
voxel_mapper(voxel_size=0.1),
cost_mapper(),
wavefront_frontier_explorer(),
# Visualization
websocket_vis(),
foxglove_bridge(),
)
.global_config(n_dask_workers=4, robot_model="unitree_g1")
.transports(
{
# G1 uses Twist for movement commands
("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist),
# State estimation from ROS
("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry),
# Odometry output from ROSNavigationModule
("odom", PoseStamped): LCMTransport("/odom", PoseStamped),
# Navigation module topics from nav_bot
("goal_req", PoseStamped): LCMTransport("/goal_req", PoseStamped),
("goal_active", PoseStamped): LCMTransport("/goal_active", PoseStamped),
("path_active", Path): LCMTransport("/path_active", Path),
("pointcloud", PointCloud2): LCMTransport("/lidar", PointCloud2),
("global_pointcloud", PointCloud2): LCMTransport("/map", PointCloud2),
# Original navigation topics for backwards compatibility
("goal_pose", PoseStamped): LCMTransport("/goal_pose", PoseStamped),
("goal_reached", Bool): LCMTransport("/goal_reached", Bool),
("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool),
# Camera topics (if camera module is added)
("color_image", Image): LCMTransport("/g1/color_image", Image),
("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo),
}
)
)

basic_ros = autoconnect(
_basic_no_nav,
g1_connection(),
ros_nav(),
)

basic_sim = autoconnect(
_basic_no_nav,
g1_sim_connection(),
replanning_a_star_planner(),
)

_perception_and_memory = autoconnect(
spatial_memory(),
object_tracking(frame_id="camera_link"),
utilization(),
)

standard = autoconnect(
basic_ros,
_perception_and_memory,
).global_config(n_dask_workers=8)

standard_sim = autoconnect(
basic_sim,
_perception_and_memory,
).global_config(n_dask_workers=8)

# Optimized configuration using shared memory for images
standard_with_shm = autoconnect(
standard.transports(
{
("color_image", Image): pSHMTransport(
"/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE
),
}
),
foxglove_bridge(
shm_channels=[
"/g1/color_image#sensor_msgs.Image",
]
),
)

_agentic_skills = autoconnect(
llm_agent(),
human_input(),
navigation_skill(),
g1_skills(),
)

# Full agentic configuration with LLM and skills
agentic = autoconnect(
standard,
_agentic_skills,
)

agentic_sim = autoconnect(
standard_sim,
_agentic_skills,
)

# Configuration with joystick control for teleoperation
with_joystick = autoconnect(
basic_ros,
keyboard_teleop(), # Pygame-based velocity control (cmd_vel)
keyboard_pose_teleop(), # Pygame-based pose goal control (goal_pose/cancel_goal)
)

# Detection configuration with person tracking and 3D detection
detection = (
autoconnect(
basic_ros,
# Person detection modules with YOLO
detection3d_module(
camera_info=zed.CameraInfo.SingleWebcam,
detector=YoloPersonDetector,
),
detectionDB_module(
camera_info=zed.CameraInfo.SingleWebcam,
filter=lambda det: det.class_id == 0, # Filter for person class only
),
person_tracker_module(
cameraInfo=zed.CameraInfo.SingleWebcam,
),
)
.global_config(n_dask_workers=8)
.remappings(
[
# Connect detection modules to camera and lidar
(Detection3DModule, "image", "color_image"),
(Detection3DModule, "pointcloud", "pointcloud"),
(ObjectDBModule, "image", "color_image"),
(ObjectDBModule, "pointcloud", "pointcloud"),
(PersonTracker, "image", "color_image"),
(PersonTracker, "detections", "detections_2d"),
]
)
.transports(
{
# Detection 3D module outputs
("detections", Detection3DModule): LCMTransport(
"/detector3d/detections", Detection2DArray
),
("annotations", Detection3DModule): LCMTransport(
"/detector3d/annotations", ImageAnnotations
),
("scene_update", Detection3DModule): LCMTransport(
"/detector3d/scene_update", SceneUpdate
),
("detected_pointcloud_0", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/0", PointCloud2
),
("detected_pointcloud_1", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/1", PointCloud2
),
("detected_pointcloud_2", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/2", PointCloud2
),
("detected_image_0", Detection3DModule): LCMTransport("/detector3d/image/0", Image),
("detected_image_1", Detection3DModule): LCMTransport("/detector3d/image/1", Image),
("detected_image_2", Detection3DModule): LCMTransport("/detector3d/image/2", Image),
# Detection DB module outputs
("detections", ObjectDBModule): LCMTransport(
"/detectorDB/detections", Detection2DArray
),
("annotations", ObjectDBModule): LCMTransport(
"/detectorDB/annotations", ImageAnnotations
),
("scene_update", ObjectDBModule): LCMTransport("/detectorDB/scene_update", SceneUpdate),
("detected_pointcloud_0", ObjectDBModule): LCMTransport(
"/detectorDB/pointcloud/0", PointCloud2
),
("detected_pointcloud_1", ObjectDBModule): LCMTransport(
"/detectorDB/pointcloud/1", PointCloud2
),
("detected_pointcloud_2", ObjectDBModule): LCMTransport(
"/detectorDB/pointcloud/2", PointCloud2
),
("detected_image_0", ObjectDBModule): LCMTransport("/detectorDB/image/0", Image),
("detected_image_1", ObjectDBModule): LCMTransport("/detectorDB/image/1", Image),
("detected_image_2", ObjectDBModule): LCMTransport("/detectorDB/image/2", Image),
# Person tracker outputs
("target", PersonTracker): LCMTransport("/person_tracker/target", PoseStamped),
}
)
)

# Full featured configuration with everything
full_featured = autoconnect(
standard_with_shm,
_agentic_skills,
keyboard_teleop(),
)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Duplicates existing modular blueprint structure

This file re-declares all existing G1 blueprints (basic_ros, basic_sim, standard, agentic, detection, with_joystick, full_featured, etc.) in a single monolithic file, duplicating what already exists across the modular subdirectory structure at:

  • basic/unitree_g1_basic.py, basic/unitree_g1_basic_sim.py, basic/unitree_g1_joystick.py
  • agentic/unitree_g1_agentic.py, agentic/unitree_g1_agentic_sim.py, agentic/unitree_g1_full.py
  • perceptive/unitree_g1.py, perceptive/unitree_g1_detection.py, perceptive/unitree_g1_shm.py
  • primitive/uintree_g1_primitive_no_nav.py

The __init__.py in this directory already uses lazy_loader to expose these blueprints from the subdirectories — and it has no reference to this new file. As a result, this file is effectively dead code unless someone explicitly imports from dimos.robot.unitree.g1.blueprints.unitree_g1_blueprints.

The goal of adding keyboard_pose_teleop() to the joystick blueprint would be better achieved by modifying the existing basic/unitree_g1_joystick.py (which is only 6 lines of substance), and updating the __init__.py if needed. That avoids two competing definitions of the same blueprints that will inevitably drift.

christiefhyang and others added 2 commits March 14, 2026 14:51
Co-authored-by: greptile-apps[bot] <165735046+greptile-apps[bot]@users.noreply.github.com>
@christiefhyang christiefhyang marked this pull request as draft March 14, 2026 07:20
Co-authored-by: greptile-apps[bot] <165735046+greptile-apps[bot]@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant